#include "Point.h"
#include <cmath>


double squared_distance(const Point& p, const Point& q)
{
	assert (p.dimension() == q.dimension());

	double sq_d(0);
	for (int i(0); i < p.dimension(); ++i)
	{
		double delta = p[i] - q[i];
		sq_d += delta*delta; 
	}
	return sq_d;
}
double distance_points(const Point& p, const Point& q)
{
	return std::sqrt(squared_distance(p, q));
}

double smallest_angle(const Point &s, const Point &t)
{
	// source: http://stackoverflow.com/questions/1878907/the-smallest-difference-between-2-angles
	double a = t[2] - s[2] + 0.5;
	if (a > 1)
		a -= 1;
	else if (a < -1)
		a += 1;

	a -= 0.5;
	return a;
}

double angle_of_line(const Point &a, const Point &b)
{
	//finding the angle of lina a->b
	double x = b[0]-a[0];
	double y = b[1]-a[1];
	double angle = 0;
	if(y==0)
	{
		angle = x<0?0.5:0;
	}
	else 
	{
		if(x==0)
		{
			angle = y>0?0.25:-0.25;
		}
		else 
		{
			angle  = atan2(y,x)/(2*PI);
			/*if(x>0)
			{
				angle += y>0?0:0.75;//-0.25;
			}
			else 
			{
				angle += y>0?0.25:0.5;
			}*/
		}
	}
	//check if nessery
	/*int coin = (int) rand() % 2;
	if(coin==1)
	{
		angle = angle > 0.5?(angle-1):angle;
		angle  = angle < -0.5?(angle+1):angle;
	}*/
	angle = angle >= 0.25? angle-0.5:angle;
	angle = angle <= -0.25? angle+0.5:angle;

	return angle;
}